home *** CD-ROM | disk | FTP | other *** search
/ Tech Arsenal 1 / Tech Arsenal (Arsenal Computer).ISO / tek-19 / iritsm3s.zip / GEOMAT3D.C < prev    next >
C/C++ Source or Header  |  1992-01-25  |  32KB  |  902 lines
  1. /*****************************************************************************
  2. *   "Irit" - the 3d polygonal solid modeller.                     *
  3. *                                         *
  4. * Written by:  Gershon Elber                Ver 0.2, Mar. 1990   *
  5. ******************************************************************************
  6. *   Routines to generate transformation matrices for Translation, Rotation   *
  7. * and Scaling for 3D universe.                              *
  8. *   Routines to manipulate 3D vectors.                         *
  9. *   And some computational geometry routines.                     *
  10. *****************************************************************************/
  11.  
  12. #include <math.h>
  13. #include <stdio.h>
  14. #include "program.h"
  15. #include "allocate.h"
  16. #include "convex.h"
  17. #include "geomat3d.h"
  18. #include "objects.h"
  19. #include "primitiv.h"
  20. #include "windows.h"
  21. #include "graphgen.h"
  22.  
  23. /* #define DEBUG        Print information on entry and exit of routines. */
  24.  
  25. static int CGPointRayRelation(PointType Pt, PointType PtRay, int Axes);
  26.  
  27. /*****************************************************************************
  28. * Routine to generate a 4*4 unit matrix:                                     *
  29. *****************************************************************************/
  30. void MatGenUnitMat(MatrixType Mat)
  31. {
  32.     int i, j;
  33.  
  34.     for (i = 0; i < 4; i++) for (j = 0; j < 4; j++)
  35.     if (i == j)
  36.         Mat[i][j] = 1.0;
  37.     else
  38.         Mat[i][j] = 0.0;
  39. }
  40.  
  41. /*****************************************************************************
  42. * Routine to generate a 4*4 matrix to translate in Tx, Ty, Tz amounts.       *
  43. *****************************************************************************/
  44. void MatGenMatTrans(RealType Tx, RealType Ty, RealType Tz, MatrixType Mat)
  45. {
  46.      MatGenUnitMat(Mat);                             /* Make it unit matrix. */
  47.      Mat[3][0] = Tx;
  48.      Mat[3][1] = Ty;
  49.      Mat[3][2] = Tz;
  50. }
  51.  
  52. /*****************************************************************************
  53. * Routine to generate a 4*4 matrix to Scale x, y, z in Sx, Sy, Sz amounts.   *
  54. *****************************************************************************/
  55. void MatGenMatScale(RealType Sx, RealType Sy, RealType Sz, MatrixType Mat)
  56. {
  57.      MatGenUnitMat(Mat);                             /* Make it unit matrix. */
  58.      Mat[0][0] = Sx;
  59.      Mat[1][1] = Sy;
  60.      Mat[2][2] = Sz;
  61. }
  62.  
  63. /*****************************************************************************
  64. * Routine to generate a 4*4 matrix to Rotate around X with angle of Teta.    *
  65. * Note Teta must be given in radians.                                        *
  66. *****************************************************************************/
  67. void MatGenMatRotX1(RealType Teta, MatrixType Mat)
  68. {
  69.     RealType CTeta, STeta;
  70.  
  71.     CTeta = (RealType) cos((double) Teta);
  72.     STeta = (RealType) sin((double) Teta);
  73.     MatGenMatRotX(CTeta, STeta, Mat);
  74. }
  75.  
  76. /*****************************************************************************
  77. * Routine to generate a 4*4 matrix to Rotate around X with angle of Teta.    *
  78. *****************************************************************************/
  79. void MatGenMatRotX(RealType CosTeta, RealType SinTeta, MatrixType Mat)
  80. {
  81.      MatGenUnitMat(Mat);                             /* Make it unit matrix. */
  82.      Mat[1][1] = CosTeta;
  83.      Mat[1][2] = SinTeta;
  84.      Mat[2][1] = -SinTeta;
  85.      Mat[2][2] = CosTeta;
  86. }
  87.  
  88. /*****************************************************************************
  89. * Routine to generate a 4*4 matrix to Rotate around Y with angle of Teta.    *
  90. * Note Teta must be given in radians.                                        *
  91. *****************************************************************************/
  92. void MatGenMatRotY1(RealType Teta, MatrixType Mat)
  93. {
  94.     RealType CTeta, STeta;
  95.  
  96.     CTeta = (RealType) cos((double) Teta);
  97.     STeta = (RealType) sin((double) Teta);
  98.     MatGenMatRotY(CTeta, STeta, Mat);
  99. }
  100.  
  101. /*****************************************************************************
  102. * Routine to generate a 4*4 matrix to Rotate around Y with angle of Teta.    *
  103. *****************************************************************************/
  104. void MatGenMatRotY(RealType CosTeta, RealType SinTeta, MatrixType Mat)
  105. {
  106.      MatGenUnitMat(Mat);                             /* Make it unit matrix. */
  107.      Mat[0][0] = CosTeta;
  108.      Mat[0][2] = -SinTeta;
  109.      Mat[2][0] = SinTeta;
  110.      Mat[2][2] = CosTeta;
  111. }
  112.  
  113. /*****************************************************************************
  114. * Routine to generate a 4*4 matrix to Rotate around Z with angle of Teta.    *
  115. * Note Teta must be given in radians.                                        *
  116. *****************************************************************************/
  117. void MatGenMatRotZ1(RealType Teta, MatrixType Mat)
  118. {
  119.     RealType CTeta, STeta;
  120.  
  121.     CTeta = (RealType) cos((double) Teta);
  122.     STeta = (RealType) sin((double) Teta);
  123.     MatGenMatRotZ(CTeta, STeta, Mat);
  124. }
  125.  
  126. /*****************************************************************************
  127. * Routine to generate a 4*4 matrix to Rotate around Z with angle of Teta.    *
  128. *****************************************************************************/
  129. void MatGenMatRotZ(RealType CosTeta, RealType SinTeta, MatrixType Mat)
  130. {
  131.      MatGenUnitMat(Mat);                             /* Make it unit matrix, */
  132.      Mat[0][0] = CosTeta;
  133.      Mat[0][1] = SinTeta;
  134.      Mat[1][0] = -SinTeta;
  135.      Mat[1][1] = CosTeta;
  136. }
  137.  
  138. /*****************************************************************************
  139. * Routine to multiply 2 4by4 matrices:                                       *
  140. * Note MatRes might be one of Mat1 or Mat2 - it is only updated in the end!  *
  141. *****************************************************************************/
  142. void MatMultTwo4by4(MatrixType MatRes, MatrixType Mat1, MatrixType Mat2)
  143. {
  144.     int i, j, k;
  145.     MatrixType MatResTemp;
  146.  
  147.     for (i = 0; i < 4; i++)
  148.     for (j = 0; j < 4; j++) {
  149.        MatResTemp[i][j] = 0;
  150.        for (k = 0; k < 4; k++)
  151.            MatResTemp[i][j] += Mat1[i][k] * Mat2[k][j];
  152.     }
  153.     for (i = 0; i < 4; i++)
  154.     for (j = 0; j < 4; j++)
  155.         MatRes[i][j] = MatResTemp[i][j];
  156. }
  157.  
  158. /*****************************************************************************
  159. * Routine to add 2 4by4 matrices:                         *
  160. * Note MatRes might be one of Mat1 or Mat2.                     *
  161. *****************************************************************************/
  162. void MatAddTwo4by4(MatrixType MatRes, MatrixType Mat1, MatrixType Mat2)
  163. {
  164.     int i, j;
  165.  
  166.     for (i = 0; i < 4; i++)
  167.         for (j = 0; j < 4; j++)
  168.             MatRes[i][j] = Mat1[i][j] + Mat2[i][j];
  169. }
  170.  
  171. /*****************************************************************************
  172. * Routine to subtract 2 4by4 matrices:                         *
  173. * Note MatRes might be one of Mat1 or Mat2.                     *
  174. *****************************************************************************/
  175. void MatSubTwo4by4(MatrixType MatRes, MatrixType Mat1, MatrixType Mat2)
  176. {
  177.     int i, j;
  178.  
  179.     for (i = 0; i < 4; i++)
  180.         for (j = 0; j < 4; j++)
  181.         MatRes[i][j] = Mat1[i][j] - Mat2[i][j];
  182. }
  183.  
  184. /*****************************************************************************
  185. * Routine to scale a 4by4 matrix by constant:                     *
  186. * Note MatRes might be Mat.                             *
  187. *****************************************************************************/
  188. void MatScale4by4(MatrixType MatRes, MatrixType Mat, RealType *Scale)
  189. {
  190.     int i, j;
  191.  
  192.     for (i = 0; i < 4; i++)
  193.         for (j = 0; j < 4; j++)
  194.         MatRes[i][j] = Mat[i][j] * (*Scale);
  195. }
  196.  
  197. /*****************************************************************************
  198. * Routine to multiply Vector by 4by4 matrix:                                 *
  199. * The Vector has only 3 components (X, Y, Z) and it is assumed that W = 1    *
  200. * Note VRes might be Vec as it is only updated in the end.                   *
  201. *****************************************************************************/
  202. void MatMultVecby4by4(VectorType VRes, VectorType Vec, MatrixType Mat)
  203. {
  204.     int i, j;
  205.     RealType
  206.     CalcW = Mat[3][3];
  207.     VectorType VTemp;
  208.  
  209.     for (i = 0; i < 3; i++) {
  210.     VTemp[i] = Mat[3][i];         /* Initiate it with the weight factor. */
  211.     for (j = 0; j < 3; j++) VTemp[i] += Vec[j] * Mat[j][i];
  212.     }
  213.  
  214.     for (i = 0; i < 3; i++) CalcW += Vec[i] * Mat[i][3];
  215.     if (CalcW == 0) CalcW = 1/INFINITY;
  216.  
  217.     for (i = 0; i < 3; i++) VRes[i] = VTemp[i]/CalcW;
  218. }
  219.  
  220. /*****************************************************************************
  221. * Procedure to calculate the INVERSE of a given matrix M which is not        *
  222. * modified. The matrix is assumed to be 4 by 4 (transformation matrix).      *
  223. * Return TRUE if inverted matrix (InvM) do exists.                 *
  224. *****************************************************************************/
  225. int MatInverseMatrix(MatrixType M, MatrixType InvM)
  226. {
  227.     MatrixType A;
  228.     int i, j, k;
  229.     RealType V;
  230.  
  231.     MAT_COPY(A, M);            /* Prepare temporary copy of M in A. */
  232.     MatGenUnitMat(InvM);                 /* Make it unit matrix. */
  233.  
  234.     for (i = 0; i < 4; i++) {
  235.     V = A[i][i];                      /* Find the new pivot. */
  236.     k = i;
  237.     for (j = i + 1; j < 4; j++)
  238.         if (ABS(A[j][i]) > ABS(V)) {
  239.             /* Find maximum on col i, row i+1..n */
  240.             V = A[j][i];
  241.             k = j;
  242.         }
  243.     j = k;
  244.  
  245.     if (i != j)
  246.         for (k = 0; k < 4; k++) {
  247.         SWAP(RealType, A[i][k], A[j][k]);
  248.         SWAP(RealType, InvM[i][k], InvM[j][k]);
  249.             }
  250.  
  251.     for (j = i + 1; j < 4; j++) {     /* Eliminate col i from row i+1..n. */
  252.             V = A[j][i] / A[i][i];
  253.         for (k = 0; k < 4; k++) {
  254.                 A[j][k]    -= V * A[i][k];
  255.                 InvM[j][k] -= V * InvM[i][k];
  256.         }
  257.     }
  258.     }
  259.  
  260.     for (i = 3; i >= 0; i--) {                   /* Back Substitution. */
  261.     if (A[i][i] == 0) return FALSE;                   /* Error. */
  262.  
  263.     for (j = 0; j < i; j++) {     /* Eliminate col i from row 1..i-1. */
  264.             V = A[j][i] / A[i][i];
  265.         for (k = 0; k < 4; k++) {
  266.                 /* A[j][k] -= V * A[i][k]; */
  267.                 InvM[j][k] -= V * InvM[i][k];
  268.         }
  269.     }
  270.     }
  271.  
  272.     for (i = 0; i < 4; i++)            /* Normalize the inverse Matrix. */
  273.     for (j = 0; j < 4; j++)
  274.             InvM[i][j] /= A[i][i];
  275.  
  276.     return TRUE;
  277. }
  278.  
  279. /*****************************************************************************
  280. *  Routine to copy one vector to another:                     *
  281. *****************************************************************************/
  282. void VecCopy(VectorType Vdst, VectorType Vsrc)
  283. {
  284.     int i;
  285.  
  286.     for (i = 0; i < 3; i++)
  287.     Vdst[i] = Vsrc[i];
  288. }
  289.  
  290. /*****************************************************************************
  291. *  Routine to normalize the vector length to a unit length:                  *
  292. *****************************************************************************/
  293. void VecNormalize(VectorType V)
  294. {
  295.     int i;
  296.     RealType Len;
  297.  
  298.     Len = VecLength(V);
  299.     if (Len > 0.0)
  300.         for (i = 0; i < 3; i++) {
  301.         V[i] /= Len;
  302.         if (ABS(V[i]) < EPSILON) V[i] = 0.0;
  303.     }
  304. }
  305.  
  306. /*****************************************************************************
  307. *  Routine to calculate the magnitude (length) of a given 3D vector:         *
  308. *****************************************************************************/
  309. RealType VecLength(VectorType V)
  310. {
  311.     return sqrt(SQR(V[0]) + SQR(V[1]) + SQR(V[2]));
  312. }
  313.  
  314. /*****************************************************************************
  315. *  Routine to calculate the cross product of two vectors:                    *
  316. * Note Vres might be the same as V1 or V2 !                                  *
  317. *****************************************************************************/
  318. void VecCrossProd(VectorType Vres, VectorType V1, VectorType V2)
  319. {
  320.     VectorType Vtemp;
  321.  
  322.     Vtemp[0] = V1[1] * V2[2] - V2[1] * V1[2];
  323.     Vtemp[1] = V1[2] * V2[0] - V2[2] * V1[0];
  324.     Vtemp[2] = V1[0] * V2[1] - V2[0] * V1[1];
  325.  
  326.     VecCopy(Vres, Vtemp);
  327. }
  328.  
  329. /*****************************************************************************
  330. *  Routine to calculate the dot product of two vectors:                      *
  331. *****************************************************************************/
  332. RealType VecDotProd(VectorType V1, VectorType V2)
  333. {
  334.     return  V1[0] * V2[0] + V1[1] * V2[1] + V1[2] * V2[2];
  335. }
  336.  
  337. /*****************************************************************************
  338. * Routine to generate rotation object around the X axis in Degree degrees:   *
  339. *****************************************************************************/
  340. ObjectStruct *GenMatObjectRotX(RealType *Degree)
  341. {
  342.     MatrixType Mat;
  343.  
  344.     MatGenMatRotX1(DEG2RAD(*Degree), Mat);    /* Generate the trans. matrix. */
  345.  
  346.     return GenMatObject("", Mat, NULL);
  347. }
  348.  
  349. /*****************************************************************************
  350. * Routine to generate rotation object around the Y axis in Degree degrees:   *
  351. *****************************************************************************/
  352. ObjectStruct *GenMatObjectRotY(RealType *Degree)
  353. {
  354.     MatrixType Mat;
  355.  
  356.     MatGenMatRotY1(DEG2RAD(*Degree), Mat);    /* Generate the trans. matrix. */
  357.  
  358.     return GenMatObject("", Mat, NULL);
  359. }
  360.  
  361. /*****************************************************************************
  362. * Routine to generate rotation object around the Z axis in Degree degrees:   *
  363. *****************************************************************************/
  364. ObjectStruct *GenMatObjectRotZ(RealType *Degree)
  365. {
  366.     MatrixType Mat;
  367.  
  368.     MatGenMatRotZ1(DEG2RAD(*Degree), Mat);    /* Generate the trans. matrix. */
  369.  
  370.     return GenMatObject("", Mat, NULL);
  371. }
  372.  
  373. /*****************************************************************************
  374. * Routine to generate translation object according to XYZ vector Vec.         *
  375. *****************************************************************************/
  376. ObjectStruct * GenMatObjectTrans(VectorType Vec)
  377. {
  378.     MatrixType Mat;
  379.  
  380.     /* Generate the transformation matrix */
  381.     MatGenMatTrans(Vec[0], Vec[1], Vec[2], Mat);
  382.  
  383.     return GenMatObject("", Mat, NULL);
  384. }
  385.  
  386. /*****************************************************************************
  387. * Routine to scale an object according to XYZ scaling vector Vec.         *
  388. *****************************************************************************/
  389. ObjectStruct * GenMatObjectScale(VectorType Vec)
  390. {
  391.     MatrixType Mat;
  392.  
  393.     /* Generate the transformation matrix */
  394.     MatGenMatScale(Vec[0], Vec[1], Vec[2], Mat);
  395.  
  396.     return GenMatObject("", Mat, NULL);
  397. }
  398.  
  399. /*****************************************************************************
  400. * Routine to transform an object according to the transformation matrix.     *
  401. *****************************************************************************/
  402. ObjectStruct *TransformObject(ObjectStruct *PObj, MatrixType Mat)
  403. {
  404.     int i, j;
  405.     ObjectStruct *NewPObj;
  406.     CagdMatStruct CagdMat;
  407.  
  408.     if (IS_POLY_OBJ(PObj)) {
  409.         int IsPolygon = !IS_POLYLINE_OBJ(PObj);
  410.         VectorType Pin, PTemp;
  411.         PolygonStruct *Pl;
  412.         VertexStruct *V, *VFirst;
  413.  
  414.         NewPObj = CopyObject(NULL, PObj, FALSE);
  415.     Pl = NewPObj -> U.Pl.P;
  416.  
  417.     while (Pl != NULL) {
  418.         V = VFirst = Pl -> V;
  419.         PT_ADD(Pin, V -> Pt, Pl -> Plane);     /* Prepare point IN side. */
  420.         MatMultVecby4by4(Pin, Pin, Mat); /* Transformed relative to new. */
  421.  
  422.         do {
  423.         if (IsPolygon) PT_ADD(PTemp, V -> Pt, V -> Normal);
  424.  
  425.         MatMultVecby4by4(V -> Pt, V -> Pt, Mat); /* Update position. */
  426.  
  427.         if (IsPolygon) {
  428.             MatMultVecby4by4(PTemp, PTemp, Mat);   /* Update normal. */
  429.             PT_SUB(V -> Normal, PTemp, V -> Pt);
  430.             PT_NORMALIZE(V -> Normal);
  431.         }
  432.  
  433.         V = V -> Pnext;
  434.         }
  435.         while (V != VFirst && V != NULL);           /* VList is circular! */
  436.  
  437.         if (IsPolygon)
  438.         UpdatePolyPlane(Pl, Pin);/* Update plane eqn. using IN point.*/
  439.  
  440.         Pl = Pl -> Pnext;
  441.     }
  442.     }
  443.     else if (IS_SRF_OBJ(PObj)) {
  444.         NewPObj = CopyObject(NULL, PObj, FALSE);
  445.  
  446.     for (i = 0; i < 4; i++)
  447.             for (j = 0; j < 4; j++)
  448.         CagdMat.Mat[i][j] = Mat[i][j];
  449.  
  450.         CagdSrfMatTransform(NewPObj -> U.Srf.Srf, &CagdMat);
  451.     }
  452.     else if (IS_CRV_OBJ(PObj)) {
  453.         NewPObj = CopyObject(NULL, PObj, FALSE);
  454.  
  455.     for (i = 0; i < 4; i++)
  456.             for (j = 0; j < 4; j++)
  457.         CagdMat.Mat[i][j] = Mat[i][j];
  458.  
  459.         CagdCrvMatTransform(NewPObj -> U.Crv.Crv, &CagdMat);
  460.     }
  461.     else if (IS_OLST_OBJ(PObj)) {
  462.     NewPObj = AllocObject("", OBJ_LIST_OBJ, NULL);
  463.  
  464.         for (i = 0; PObj -> U.PObjList[i] != NULL && i < MAX_OBJ_LIST; i++) {
  465.             NewPObj -> U.PObjList[i] = TransformObject(PObj -> U.PObjList[i],
  466.                                Mat);
  467.         }
  468.         NewPObj -> U.PObjList[i] = NULL;
  469.     }
  470.     else {
  471.     WndwInputWindowPutStr("None transformable object ignored.");
  472.         NewPObj = CopyObject(NULL, PObj, FALSE);
  473.     }
  474.  
  475.     return NewPObj;
  476. }
  477.  
  478. /*****************************************************************************
  479. *   Routine to calc the distance between two 3d points                       *
  480. *****************************************************************************/
  481. RealType CGDistPointPoint(PointType P1, PointType P2)
  482. {
  483.     RealType t;
  484.  
  485. #ifdef DEBUG
  486.     printf("CGDistPointPoint entered\n");
  487. #endif /* DEBUG */
  488.  
  489.     t = sqrt(SQR(P2[0]-P1[0]) + SQR(P2[1]-P1[1]) + SQR(P2[2]-P1[2]));
  490.  
  491. #ifdef DEBUG
  492.     printf("CGDistPointPoint exit\n");
  493. #endif /* DEBUG */
  494.  
  495.      return t;
  496. }
  497.  
  498. /*****************************************************************************
  499. *   Routine to create the plane from given 3 points. if two of the points    *
  500. * are the same it returns FALSE, otherwise (succesfull) returns TRUE.         *
  501. *****************************************************************************/
  502. int CGPlaneFrom3Points(PlaneType Plane, PointType Pt1, PointType Pt2,
  503.                             PointType Pt3)
  504. {
  505.     VectorType V1, V2;
  506.  
  507. #ifdef DEBUG
  508.     printf("CGPlaneFrom3Points entered\n");
  509. #endif /* DEBUG */
  510.  
  511.     if (PT_EQ(Pt1, Pt2) || PT_EQ(Pt2, Pt3) || PT_EQ(Pt1, Pt3)) return FALSE;
  512.  
  513.     PT_SUB(V1, Pt2, Pt1);
  514.     PT_SUB(V2, Pt3, Pt2);
  515.     VecCrossProd(Plane, V1, V2);
  516.     PT_NORMALIZE(Plane);
  517.  
  518.     Plane[3] = -DOT_PROD(Plane, Pt1);
  519.  
  520. #ifdef DEBUG
  521.     printf("CGPlaneFrom3Points exit\n");
  522. #endif /* DEBUG */
  523.  
  524.     return TRUE;
  525. }
  526.  
  527. /*****************************************************************************
  528. *   Routine to calc the closest 3d point to a given 3d line. the line is     *
  529. * given as a point on it (Pl) and vector (Vl).                               *
  530. *****************************************************************************/
  531. void CGPointFromPointLine(PointType Point, PointType Pl, PointType Vl,
  532.                             PointType ClosestPoint)
  533. {
  534.     int i;
  535.     PointType V1, V2;
  536.     RealType CosAlfa, VecMag;
  537.  
  538. #ifdef DEBUG
  539.     printf("CGPointFromLinePlane entered\n");
  540. #endif /* DEBUG */
  541.  
  542.     for (i = 0; i < 3; i++) {
  543.         V1[i] = Point[i] - Pl[i];
  544.         V2[i] = Vl[i];
  545.     }
  546.     VecMag = VecLength(V1);
  547.     VecNormalize(V1); /* Normalized vector from Point to a point on line Pl. */
  548.     VecNormalize(V2);                   /* Normalized line direction vector. */
  549.  
  550.     CosAlfa = VecDotProd(V1, V2); /* Find the angle between the two vectors. */
  551.  
  552.     /* Find P1 - the closest point to Point on the line: */
  553.     for (i = 0; i < 3; i++)
  554.     ClosestPoint[i] = Pl[i] + V2[i] * CosAlfa * VecMag;
  555.  
  556. #ifdef DEBUG
  557.     printf("CGPointFromLinePlane exit\n");
  558. #endif /* DEBUG */
  559. }
  560.  
  561. /*****************************************************************************
  562. *   Routine to calc the distance between 3d point and 3d line. the line is   *
  563. * given as a point on it (Pl) and vector (Vl).                               *
  564. *****************************************************************************/
  565. RealType CGDistPointLine(PointType Point, PointType Pl, PointType Vl)
  566. {
  567.     RealType t;
  568.     PointType Ptemp;
  569.  
  570. #ifdef DEBUG
  571.     printf("CGDistPointLine entered\n");
  572. #endif /* DEBUG */
  573.  
  574.     CGPointFromPointLine(Point, Pl, Vl, Ptemp);/* Find closest point on line.*/
  575.     t = CGDistPointPoint(Point, Ptemp);
  576.  
  577. #ifdef DEBUG
  578.     printf("CGDistPointLine exit\n");
  579. #endif /* DEBUG */
  580.  
  581.     return t;
  582. }
  583.  
  584. /*****************************************************************************
  585. *   Routine to calc the distance between a Point and a Plane. The Plane is   *
  586. * defined with 4 coef. : Ax + By + Cz + D = 0 given as 4 elements vector.    *
  587. *****************************************************************************/
  588. RealType CGDistPointPlane(PointType Point, PlaneType Plane)
  589. {
  590.     RealType t;
  591.  
  592. #ifdef DEBUG
  593.     printf("CGDistPointPlane entered\n");
  594. #endif /* DEBUG */
  595.  
  596.     t = ((Plane[0] * Point [0] +
  597.       Plane[1] * Point [1] +
  598.       Plane[2] * Point [2] +
  599.       Plane[3]) /
  600.      sqrt(SQR(Plane[0]) + SQR(Plane[1]) + SQR(Plane[2])));
  601.  
  602. #ifdef DEBUG
  603.     printf("CGDistPointPlane exit\n");
  604. #endif /* DEBUG */
  605.  
  606.     return t;
  607. }
  608.  
  609. /*****************************************************************************
  610. *   Routine to find the intersection point of a line and a plane (if any)    *
  611. *   The Plane is defined with 4 coef. : Ax + By + Cz + D = 0 given as 4      *
  612. * elements vector. The line is define via a point on it Pl and a direction   *
  613. * vector Vl. Return TRUE only if such point exists.                          *
  614. *****************************************************************************/
  615. int CGPointFromLinePlane(PointType Pl, PointType Vl, PlaneType Plane,
  616.                     PointType InterPoint, RealType *t)
  617. {
  618.     int i;
  619.     RealType DotProd;
  620.  
  621. #ifdef DEBUG
  622.     printf("CGPointFromLinePlane entered\n");
  623. #endif /* DEBUG */
  624.  
  625.     /* Check to see if they are vertical - no intersection at all! */
  626.     DotProd = VecDotProd(Vl, Plane);
  627.     if (ABS(DotProd) < EPSILON) return FALSE;
  628.  
  629.     /* Else find t in line such that the plane equation plane is satisfied: */
  630.     *t = (-Plane[3] - Plane[0] * Pl[0] - Plane[1] * Pl[1] - Plane[2] * Pl[2])
  631.                                 / DotProd;
  632.  
  633.     /* And so find the intersection point which is at that t: */
  634.     for (i = 0; i < 3; i++) InterPoint[i] = Pl[i] + *t * Vl[i];
  635.  
  636. #ifdef DEBUG
  637.     printf("CGPointFromLinePlane exit\n");
  638. #endif /* DEBUG */
  639.  
  640.     return TRUE;
  641. }
  642.  
  643. /*****************************************************************************
  644. *   Routine to find the intersection point of a line and a plane (if any)    *
  645. *   The Plane is defined with 4 coef. : Ax + By + Cz + D = 0 given as 4      *
  646. * elements vector. The line is define via a point on it Pl and a direction   *
  647. * vector Vl: Line == Pl + Vl * t, where t is the line parameter.         *
  648. *   Return TRUE only if such point exists, in the t parameter range [0..1].  *
  649. *****************************************************************************/
  650. int CGPointFromLinePlane01(PointType Pl, PointType Vl, PlaneType Plane,
  651.                     PointType InterPoint, RealType *t)
  652. {
  653.     int i;
  654.     RealType DotProd;
  655.  
  656. #ifdef DEBUG
  657.     printf("CGPointFromLinePlane01 entered\n");
  658. #endif /* DEBUG */
  659.  
  660.     /* Check to see if they are vertical - no intersection at all! */
  661.     DotProd = VecDotProd(Vl, Plane);
  662.     if (ABS(DotProd) < EPSILON) return FALSE;
  663.  
  664.     /* Else find t in line such that the plane equation plane is satisfied: */
  665.     *t = (-Plane[3] - Plane[0] * Pl[0] - Plane[1] * Pl[1] - Plane[2] * Pl[2])
  666.                                 / DotProd;
  667.  
  668.     if ((*t < 0.0 && !APX_EQ(*t, 0.0)) ||      /* Not in parameter range. */
  669.     (*t > 1.0 && !APX_EQ(*t, 1.0))) return FALSE;
  670.  
  671.     /* And so find the intersection point which is at that t : */
  672.     for (i = 0; i < 3; i++) InterPoint[i] = Pl[i] + *t * Vl[i];
  673.  
  674. #ifdef DEBUG
  675.     printf("CGPointFromLinePlane01 exit\n");
  676. #endif /* DEBUG */
  677.  
  678.     return TRUE;
  679. }
  680.  
  681. /*****************************************************************************
  682. *   Routine to find the two point Pti on the lines (Pli, Vli) ,   i = 1, 2   *
  683. * with the minimal euclidian distance between them. In other words the       *
  684. * distance between Pt1 and Pt2 is defined as distance between the two lines. *
  685. *   The two points are calculated using the fact that if V = (Vl1 cross Vl2) *
  686. * then these two points are the intersection point between the following:    *
  687. * point 1 - a plane (defined by V and line1) and the line line2.             *
  688. * point 2 - a plane (defined by V and line2) and the line line1.             *
  689. *   This function returns TRUE iff the two lines are not parallel!           *
  690. *****************************************************************************/
  691. int CG2PointsFromLineLine(PointType Pl1, PointType Vl1,
  692.               PointType Pl2, PointType Vl2,
  693.               PointType Pt1, RealType *t1,
  694.               PointType Pt2, RealType *t2)
  695. {
  696.     int i;
  697.     PointType Vtemp;
  698.     PlaneType Plane1, Plane2;
  699.  
  700. #ifdef DEBUG
  701.     printf("CG2PointsFromLineLine entered\n");
  702. #endif /* DEBUG */
  703.  
  704.     VecCrossProd(Vtemp, Vl1, Vl2);     /* Check to see if they are parallel. */
  705.     if (VecLength(Vtemp) < EPSILON) {
  706.     for (i = 0; i < 3; i++)
  707.         Pt1[i] = Pl1[i];             /* Pick point on line1 and find */
  708.     CGPointFromPointLine(Pl1, Pl2, Vl2, Pt2); /* closest point on line2. */
  709.         return FALSE;
  710.     }
  711.  
  712.     /* Define the two planes: 1) Vl1, Pl1, Vtemp and 2) Vl2, Pl2, Vtemp         */
  713.     /* Note this sets the first 3 elements A, B, C out of the 4...         */
  714.     VecCrossProd(Plane1, Vl1, Vtemp);           /* Find the A, B, C coef.'s. */
  715.     VecNormalize(Plane1);
  716.     VecCrossProd(Plane2, Vl2, Vtemp);           /* Find the A, B, C coef.'s. */
  717.     VecNormalize(Plane2);
  718.  
  719.     /* and now use a point on the plane to find the 4th coef. D: */
  720.     Plane1[3] = (-VecDotProd(Plane1, Pl1)); /* Note VecDotProd uses only the */
  721.     Plane2[3] = (-VecDotProd(Plane2, Pl2)); /* first three elements in vec.  */
  722.  
  723.     /* Thats it! now we should solve for the intersection point between a    */
  724.     /* line and a plane but we already familiar with this problem...         */
  725.     i = CGPointFromLinePlane(Pl1, Vl1, Plane2, Pt1, t1) &&
  726.     CGPointFromLinePlane(Pl2, Vl2, Plane1, Pt2, t2);
  727.  
  728. #ifdef DEBUG
  729.     printf("CG2PointsFromLineLine exit\n");
  730. #endif /* DEBUG */
  731.  
  732.     return i;
  733. }
  734.  
  735. /*****************************************************************************
  736. *   Routine to find the distance between two lines (Pli, Vli) ,  i = 1, 2.   *
  737. *****************************************************************************/
  738. RealType CGDistLineLine(PointType Pl1, PointType Vl1,
  739.             PointType Pl2, PointType Vl2)
  740. {
  741.     RealType t1, t2;
  742.     PointType Ptemp1, Ptemp2;
  743.  
  744. #ifdef DEBUG
  745.     printf("CGDistLineLine entered\n");
  746. #endif /* DEBUG */
  747.  
  748.     CG2PointsFromLineLine(Pl1, Vl1, Pl2, Vl2, Ptemp1, &t1, Ptemp2, &t2);
  749.     t1 = CGDistPointPoint(Ptemp1, Ptemp2);
  750.  
  751. #ifdef DEBUG
  752.     printf("CGDistLineLine exit\n");
  753. #endif /* DEBUG */
  754.  
  755.     return t1;
  756. }
  757.  
  758. /*****************************************************************************
  759. *   Routine implements Jordan Theorem: Fire a ray from a given point and find*
  760. * number of intersections of ray with the polygon, excluding the given point *
  761. * Pt (start of ray) itself, if on polygon boundary. The ray is fired in +X   *
  762. * (Axes == 0) or +Y (Axes == 1). And only the X/Y coordinates of the polygon *
  763. * are taken into account, i.e. the orthogonal projection of the polygon on   *
  764. * a X/Y parallel plane (equal to polygon itself if on X/Y parallel plane...) *
  765. *   Note that if the point is on polygon boundary, the ray should not be in  *
  766. * its edge direction                                 *
  767. *                                         *
  768. *   Algorithm:                                     *
  769. * 1. Set NumOfIntersection = 0;                             *
  770. *    Find vertex V not on Ray level and set AlgState to its level (below or  *
  771. *    above the ray level). If none goto 3                     *
  772. *    Mark VStart = V;                                 *
  773. * 2. 2.1. While State(V) == AlgState do                         *
  774. *        2.1.1. V = V -> Pnext;                         *
  775. *        2.1.2. If V == VStart goto 3                     *
  776. *      end;                                     *
  777. *      IntersectionMinX = INFINITY;                         *
  778. *    2.2. While State(V) == ON_RAY do                         *
  779. *        IntersectionMin = MIN(IntersectionMin, V -> Pt[Axes]);         *
  780. *        V = V -> Pnext;                             *
  781. *         end;                                     *
  782. *    2.3. If State(V) != AlgState do                         *
  783. *        2.3.1. Find the intersection point between polygon edge      *
  784. *               Vlast, V and the Ray and update IntersectionMin if    *
  785. *               lower than it.                         *
  786. *        2.3.2. If IntersectionMin is greater than Pt[Axes] increase  *
  787. *               the NumOfIntersection counter by 1.             *
  788. *      end;                                     *
  789. *    2.4. AlgState = State(V);                             *
  790. *    2.5. goto 2.1.                                 *
  791. * 3. return NumOfIntersection;                             *
  792. *                                         *
  793. *****************************************************************************/
  794. int CGPolygonRayInter(PolygonStruct *Pl, PointType PtRay, int RayAxes)
  795. {
  796.     int NewState, AlgState, RayOtherAxes,
  797.     NumOfInter = 0,
  798.     Quit = FALSE;
  799.     RealType InterMin, Inter, t;
  800.     VertexStruct *V, *VStart, *VLast;
  801.  
  802.     RayOtherAxes = (RayAxes == 1 ? 0 : 1);     /* Other dir: X -> Y, Y -> X. */
  803.  
  804.     /* Stage 1 - find a vertex below the ray level: */
  805.     V = VStart = Pl -> V;
  806.     do {
  807.     if ((AlgState = CGPointRayRelation(V -> Pt, PtRay, RayOtherAxes))
  808.                             != ON_RAY) break;
  809.     V = V -> Pnext;
  810.     }
  811.     while (V != VStart && V != NULL);
  812.     if (AlgState == ON_RAY) return 0;
  813.     VStart = V; /* Vertex Below Ray level */
  814.  
  815.     /* Stage 2 - scan the vertices and count number of intersections. */
  816.     while (!Quit) {
  817.     /* Stage 2.1. : */
  818.     while (CGPointRayRelation(V -> Pt, PtRay, RayOtherAxes) == AlgState) {
  819.         VLast = V;
  820.         V = V -> Pnext;
  821.         if (V == VStart) {
  822.         Quit = TRUE;
  823.         break;
  824.         }
  825.     }
  826.     InterMin = INFINITY;
  827.  
  828.     /* Stage 2.2. : */
  829.     while (CGPointRayRelation(V -> Pt, PtRay, RayOtherAxes) == ON_RAY) {
  830.         InterMin = MIN(InterMin, V -> Pt[RayAxes]);
  831.         VLast = V;
  832.         V = V -> Pnext;
  833.         if (V == VStart) Quit = TRUE;
  834.     }
  835.  
  836.     /* Stage 2.3. : */
  837.     if ((NewState = CGPointRayRelation(V -> Pt, PtRay, RayOtherAxes))
  838.                                 != AlgState) {
  839.         /* Stage 2.3.1 Intersection with ray is in middle of edge: */
  840.         t = (PtRay[RayOtherAxes] - V -> Pt[RayOtherAxes]) /
  841.         (VLast -> Pt[RayOtherAxes] - V -> Pt[RayOtherAxes]);
  842.         Inter = VLast -> Pt[RayAxes] * t + V -> Pt[RayAxes] * (1.0 - t);
  843.         InterMin = MIN(InterMin, Inter);
  844.  
  845.         /* Stage 2.3.2. comp. with ray base and inc. # of inter if above.*/
  846.         if (InterMin > PtRay[RayAxes] &&
  847.         !APX_EQ(InterMin, PtRay[RayAxes])) NumOfInter++;
  848.     }
  849.  
  850.     AlgState = NewState;
  851.     }
  852.  
  853.     return NumOfInter;
  854. }
  855.  
  856. /*****************************************************************************
  857. *   Routine to return the relation between the ray level and a given point,  *
  858. * to be used in the CGPolygonRayInter routine above.                 *
  859. *****************************************************************************/
  860. static int CGPointRayRelation(PointType Pt, PointType PtRay, int Axes)
  861. {
  862.     if (APX_EQ(PtRay[Axes], Pt[Axes]))
  863.         return ON_RAY;
  864.     else if (PtRay[Axes] < Pt[Axes])
  865.         return ABOVE_RAY;
  866.     else
  867.     return BELOW_RAY;
  868. }
  869.  
  870.  
  871. /*****************************************************************************
  872. * Same as CGPolygonRayInter but for arbitrary oriented polygon.             *
  873. * The polygon (and the point) is first rotated to a XY parallel plane.         *
  874. *****************************************************************************/
  875. int CGPolygonRayInter3D(PolygonStruct *Pl, PointType PtRay, int RayAxes)
  876. {
  877.     int i;
  878.     MatrixType RotMat;
  879.     VertexStruct *V, *VHead;
  880.     PolygonStruct *RotPl;
  881.     PointType RotPt;
  882.  
  883.     /* Make a copy of original to work on. */
  884.     RotPl = AllocPolygon(1, Pl ->Tags, CopyVList(Pl -> V), NULL);
  885.  
  886.     /* Bring the polygon to a XY parallel plane by rotation. */
  887.     GenRotateMatrix(RotMat, Pl -> Plane);
  888.     V = VHead = RotPl -> V;
  889.     do {                    /* Transform the polygon itself. */
  890.     MatMultVecby4by4(V -> Pt, V -> Pt, RotMat);
  891.     V = V -> Pnext;
  892.     }
  893.     while (V != NULL && V != VHead);
  894.     MatMultVecby4by4(RotPt, PtRay, RotMat);
  895.  
  896.     i = CGPolygonRayInter(RotPl, RotPt, RayAxes);
  897.  
  898.     MyFree((char *) RotPl, ALLOC_POLYGON);
  899.  
  900.     return i;
  901. }
  902.